Second Order Minimum Energy Filtering on SE3 with Nonlinear Measurement Equations

Accurate camera motion estimation is a fundamental building block for many Computer Vision algorithms. For improved robustness, temporal consistency of translational and rotational camera velocity is often assumed by propagating motion information forward using stochastic filters. Classical stochastic filters, however, use linear approximations for the non-linear observer model and for the non-linear structure of the underlying Lie Group SE3 and have to approximate the unknown posteriori distribution. In this paper we employ a nonlinear measurement model for the camera motion estimation problem that incorporates multiple observation equations. We solve the underlying filtering problem using a novel Minimum Energy Filter on SE3 and give explicit expressions for the optimal state variables. Experiments on the challenging KITTI benchmark show that, although a simple motion model is only employed, our approach improves rotational velocity estimation and otherwise is on par with the state-of-the-art.